#ifndef ROBOT_DRIVE_CUSTOM_H
#define ROBOT_DRIVE_CUSTOM_H

/*
 * Drive class
 * Team 675 - Michael Gibson
 * Created January 2012
 *
 * Does Boring Stuff 
 */
#include "Include.h"
#include "DriveType.h"
#include "DoubleVictor.h"
#include "BeltSystem.h"

namespace Team675
{

class RobotDrive
{	
public:
	RobotDrive(DriveType *_drive);
	~RobotDrive();
	
	void Update();
	void SetDrive(DriveType *_type);
	
private:
	DoubleVictor  			*m_leftVictors, *m_rightVictors;
	Joystick      			*m_leftStick, *m_rightStick;
//	Encoder       			*m_leftEncoder, *m_rightEncoder;
	Solenoid      			*m_leftSolenoidOn, *m_leftSolenoidOff, *m_rightSolenoidOn, *m_rightSolenoidOff;
	SendablePIDController 	*m_leftPID, *m_rightPID ;
	Compressor    			*m_compressor;
	DriveType     			*m_drive;
	DriveType::ShiftSetting m_shiftSetting;
	DriveInfo     			m_info;
	bool          			m_shiftState;
	Timer					m_rampArmTimer;
	bool 					m_rampArmIsDown;
	bool 					m_rampServoIsIn;
	Relay					m_rampArmRelay;
	Servo					m_rampArmServo;
	
	void InitPID();
	void KillPID();
	void ShiftUpdate(DriveInfo &_info);
	void DriveUpdate(DriveInfo &_info);
	void Shift(bool _high);
	bool GetAutoShiftHigh();
};
	
}

#endif
